%Code to randomise placement of some object and then to detect the
%placement of it through MLE
%Authors: Nathan Rich Chris Chester
clear
clc
close all

noisemult = 1; %Noise multiplier

% Pre-defining Arrays
R1 = zeros(4000,1);
R2 = zeros(4000,1);
S = zeros(4000,1);
Time = zeros(4000,1);
Out1 = zeros(4000,1);
Out2 = zeros(4000,1);


a1 = 0; %Position of Antenna relative to x axis, 0 on y axis
a2 = 1e-6; %antennas 1mm apart

freqwave = 8e9;    %8GHz Cosine wave used
velowave = 3e5; %speed of light km/s
lengthwave = velowave/freqwave;

%Both antenna first transmit a cosine then wait to receive it back from
%object.
srate = 80; %80 samples per wave

%With ideal LPF

load Hd.mat;

for K = 1:20
    N(K) = K*10;%cycling through number of iterations
    
    %Creating Signal to be sent
    for I = 1:4000
        S(I) = cos(I*2*pi/srate); %Sent Signal
        Time(I) = I/(freqwave*srate)*1e9;  %Scales to ns
    end
    clear xguess yguess xguessmed yguessmed
    xguess = zeros(N(K),1);
    yguess = zeros(N(K),1);
    xguessmed = zeros(N(K),1);
    yguessmed = zeros(N(K),1);
    for J = 1:N(K)
        
        %Obtaining the recieved signal at antennas 1 and 2
        [R1, R2, delay1, delay2,objx,objy] = radar_signal(S,a1,a2,freqwave,velowave,srate,noisemult);
        
        
        %Using cos(a)cos(a+b) = 1/2 cos(b) + 1/2 cos(2a), to find cos(b)
        Out1 = filter(Hd.Numerator,1,S.*R1);
        Out2 = filter(Hd.Numerator,1,S.*R2);
        
        %Averaging the DC of the filter (%%%%%*!MLE estimate!*%%%%%%)
        Out1ave = mean(Out1(1000:4000));   %First 1000 terms ignored to allow
        Out2ave = mean(Out2(1000:4000));   %for the signal to converge
        Out1med = median(Out1(1000:4000));
        Out2med = median(Out2(1000:4000));
        if Out1ave > 0.5
            Out1ave = 0.5;
        end
        if Out2ave > 0.5
            Out2ave = 0.5;
        end
        if Out1med > 0.5
            Out1med = 0.5;
        end
        if Out2med > 0.5
            Out2med = 0.5;
        end
        
        
        %Filter has a DC gain of 0.9067 and 1/2 scaling
        phase1 = acos (Out1ave*2);
        phase2 = acos (Out2ave*2);
        phase1med = acos (Out1med*2);
        phase2med = acos (Out2med*2);
        
        %         %Checks to see if acos has chosen the right angle out of the two possible
        %         %choices
        %         if (R1(srate*1/4) < 0) %sees if the wave is -ve at the pi/2 point
        %             phase1 = 2* pi - phase1; % If the wave is -ve then the phase delay > pi
        %             phase1med = 2* pi - phase1med;
        %         end
        %
        %         if (R2(srate*1/4) < 0)
        %             phase2 = 2* pi - phase2;
        %             phase2med = 2* pi - phase2med;
        %         end
        
        %Finds distance
        phasedif = (phase1-phase2);
        
        length_from_a1 = velowave*delay1/2; %d=v*t, wave travels path twice so divide by two
        length_from_a2 = velowave*delay2 - length_from_a1;  % wave travel a1 path aswell, so minus it
        distdif = phasedif/(2*pi) * lengthwave; %d=(phase*wavelength)/(2pi) using d=vt, t*omega=phase, wavelength*f=v
        
        theta = acos(distdif/(a2-a1));
        
        xguess(J) = abs(length_from_a1*cos(theta));
        yguess(J) = abs(length_from_a1*sin(theta));
        
        %Finds median distance
        phasedifmed = (phase1med-phase2med);
        distdifmed = phasedifmed/(2*pi) * lengthwave; %d=(phase*wavelength)/(2pi) using d=vt, t*omega=phase, wavelength*f=v
        
        thetamed = acos(distdifmed/(a2-a1));
        
        xguessmed(J) = abs(length_from_a1*cos(thetamed));
        yguessmed(J) = abs(length_from_a1*sin(thetamed));
    end
    
    
    x_new(K) = abs(mean(xguess)); %%%%%%%MLE
    y_new(K) = abs(mean(yguess)); %%%%%%%MLE
    xmedian(K) = median(abs(xguessmed));
    ymedian(K) = median(abs(yguessmed));
    
    xerror(K) = abs(objx - x_new(K));
    yerror(K) = abs(objy - y_new(K));
    xmederror(K) = abs(objx - xmedian(K));
    ymederror(K) = abs(objy - ymedian(K));
    
    dist_error(K) = sqrt(xerror(K)^2 + yerror(K)^2);
    dist_errormedian(K) = sqrt(xmederror(K)^2 + ymederror(K)^2);
end


figure(1)
plot(N,dist_error, 'r')
hold on
plot(N,dist_errormedian, 'b')
title('Error with respect to noise')
xlabel('Number of iterations')
ylabel('Distance from actual')
legend('Predicted MLE','Predicted Median','Location','NorthWest')

xcoord1 = [a1, a2, objx];
ycoord1 = [0,0,objy];
xcoord2 = x_new;
ycoord2 = y_new;
xcoord3 = xmedian;
ycoord3 = ymedian;

figure(2) %scatter graph to show estimation
scatter(xcoord2, ycoord2,50,'r','x')
hold on
scatter(xcoord1, ycoord1,'b','o')
hold on
scatter(xcoord3, ycoord3,'g','+')

title('Radar Estimation')
xlabel('distance (km)')
ylabel('distance (km)')
legend('Predicted','Actual', 'Median', 'Location', 'NorthWest')

